﻿#ifndef EKF_A_H
#define EKF_A_H

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
//#include <cmath>
using namespace Eigen;
using namespace std;

class EKF_A {
public:
//    EKF_A(Vector3d r_a, Vector3d a,double t, double q, double r);
    //EKF_A()=default;
    EKF_A(Vector3d r_a = (Vector3d()<<1,0,0).finished(),
          Vector3d a = (Vector3d()<<0,0,1).finished(),
          double q = 0.002,
          Matrix3d r = Matrix3d::Identity()*0.063,
          double t = 0): r_axis(r_a),acc_init(a),sample_period(t),Q(q),R(r)
          {
            P=1;
          }
    void init(Vector3d r_a = (Vector3d()<<1,0,0).finished(),
              Vector3d a = (Vector3d()<<0,0,1).finished(),
              double q = 0.002,
              Matrix3d r = Matrix3d::Identity()*0.0063,
              double t = 0)
    {
       r_axis = r_a;
       acc_init = a;
       Q = q;
       R = r;
       sample_period = t;
       P = 1;
       is_init_ = true;
       angle = 0;
    }
    inline bool is_init(){ return is_init_; }

    inline void reset(){ angle = 0; P = 1; is_init_ = false; }
//    void predict
    void update(Vector3d acc);

    inline void set_sample_period(double t){
        sample_period = t;
    }
    inline void set_Q(double q){
        Q = q;
    }
    inline void set_R(Matrix3d r){
        R = r;
    }
    inline void set_r_axis(Vector3d r){
        r_axis = r;
    }
    inline void set_acc_init(Vector3d a){
        acc_init = a;
    }
    inline double get_angle(){
        return angle;
    }
    inline void log_out(){
        cout << "angle : "<< angle<<endl;
        cout << "r_axis : "<< r_axis<<endl;
        cout << "acc_init : "<< acc_init<<endl;
        cout <<"Q:"<< Q<<endl;
        cout <<"K:"<< K<<endl;
        cout <<"R:" <<R<<endl;
        cout << "Q:"<<Q<<endl;
        cout << "P:"<<P<<endl;
    }

private:
    double sample_period;
    double angle = 0;
    double Q;
    double P;

    Vector3d r_axis;
    Matrix3d R;
    RowVector3d K; // 1x3
    Vector3d acc_init;
//    AngleAxisd rotation_vector;
    Matrix3d rotation_matrix;
    Vector3d Hx;
    Vector3d acc_m;
    bool is_init_ = false;
};


#endif // EKF_A_H
